
lim1_min = -pi/2; lim1_max = pi/2; lim1 = lim1_max - lim1_min;
lim2_min = -pi/2; lim2_max = pi/2; lim2 = lim2_max - lim2_min;
lim3_min = -pi/2; lim3_max = pi/2; lim3 = lim3_max - lim3_min;
lim4_min = -pi/2; lim4_max = pi/2; lim4 = lim4_max - lim4_min;
lim5_min = -pi/2; lim5_max = pi/2; lim5 = lim5_max - lim5_min;

% 定义各个连杆和关节类型，默认为转动关节（可选为平移关节）
%       theta    d       a   alpha
L1=Link([   0    0       0   pi/2], 'standard'); L1.qlim = [lim1_min, lim1_max];
L2=Link([   0    0   0.045      0], 'standard'); L2.qlim = [lim2_min, lim2_max];
L3=Link([   0    0   0.045      0], 'standard'); L3.qlim = [lim3_min, lim3_max]; L2.offset = pi/2;
L4=Link([   0    0   0.045   pi/2], 'standard'); L4.qlim = [lim4_min, lim4_max];
L5=Link([   0    0       0      0], 'standard'); L5.qlim = [lim5_min, lim5_max];

robot=SerialLink([L1, L2, L3, L4, L5]); % 将5个关节组成机械臂
robot.name='5-DOF Robotic Arm';

% 定义起始点和终止点
T1 = transl(-0.135, 0, 0); % 齐次变换矩阵
T2 = transl( 0.135, 0, 0);  % 齐次变换矩阵
init_ang = robot.ikine(T1, 'mask', [1 1 1 1 1 0]); % 运动学逆解
targ_ang = robot.ikine(T2, 'mask', [1 1 1 1 1 0]); % 运动学逆解

% 定义储存机械臂关节旋转角度、角速度及角加速度的数组
% N = 5;
step = 50;
q   = zeros(step,N); % 初始化机械臂的位置
qd  = zeros(step,N); % 初始化机械臂的角速度
qdd = zeros(step,N); % 初始化机械臂的角加速度

% 对控制轨迹进行线性插值
% step = 50;
for t = 1:step
    q(t,:)   = (targ_ang-init_ang)/step * t + init_ang;  % 位置
	qd(t,:)  = (targ_ang-init_ang)/step;                 % 角速度
	qdd(t,:) = 0;                                        % 角加速度
end

T = robot.fkine(q);   % 运动学正解
nT = T.T;             % 转为齐次比那换矩阵
plot3(squeeze(nT(1,4,:)), squeeze(nT(2,4,:)), squeeze(nT(3,4,:))); % 输出末端轨迹
title('线性插值轨迹');
robot.plot(q);		% 动画演示 

